
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.templates.commands.FindCameraDistance;
import edu.wpi.first.wpilibj.templates.commands.LockBridgeArm;
import edu.wpi.first.wpilibj.templates.commands.LowerBridge;
import edu.wpi.first.wpilibj.templates.commands.PickupBallsLower;
import edu.wpi.first.wpilibj.templates.commands.RaiseBridgeArm;
import edu.wpi.first.wpilibj.templates.commands.ReversePickupLower;
import edu.wpi.first.wpilibj.templates.commands.ShooterSpin;
import edu.wpi.first.wpilibj.templates.commands.SlowForward;
import edu.wpi.first.wpilibj.templates.commands.SlowReverse;
import edu.wpi.first.wpilibj.templates.commands.StrafeSlowLeft;
import edu.wpi.first.wpilibj.templates.commands.StrafeSlowRight;
import edu.wpi.first.wpilibj.templates.commands.UnlockBridgeArm;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
    // Joystick stick = new Joystick(port);
    // Button button = new JoystickButton(stick, buttonNumber);
    
    // Another type of button you can create is a DigitalIOButton, which is
    // a button or switch hooked up to the cypress module. These are useful if
    // you want to build a customized operator interface.
    // Button button = new DigitalIOButton(1);
    
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenPressed(new ExampleCommand());
    
    // Run the command while the button is being held down and interrupt it once
    // the button is released.
    // button.whileHeld(new ExampleCommand());
    
    // Start the command when the button is released  and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenReleased(new ExampleCommand());
    
    private static OI instance = null;
    
    public static final int DRIVE_JOYSTICK_PORT = 1;
    public static final int DRIVE_JOYSTICK2_PORT = 2;
    public static final int SHOOTER_JOYSTICK_PORT = 3;

    public boolean shooterState = false;
    
    // Initialize both joysticks using the ports declared above
    Joystick driveJoystick = new Joystick(DRIVE_JOYSTICK_PORT);
    Joystick driveJoystick2 = new Joystick(DRIVE_JOYSTICK2_PORT);
    Joystick shooterJoystick = new Joystick(SHOOTER_JOYSTICK_PORT);

    // Initialize the buttons on each of the joysticks
    public Button 
            driveJs_button1 = new JoystickButton(driveJoystick, 1),
            driveJs_button2 = new JoystickButton(driveJoystick, 2),
            driveJs_button3 = new JoystickButton(driveJoystick, 3),
            driveJs_button4 = new JoystickButton(driveJoystick, 4),
            driveJs_button5 = new JoystickButton(driveJoystick, 5),
            driveJs_button6 = new JoystickButton(driveJoystick, 6),
            driveJs_button7 = new JoystickButton(driveJoystick, 7),
            driveJs_button8 = new JoystickButton(driveJoystick, 8),            
            driveJs_button10 = new JoystickButton(driveJoystick, 10),
            driveJs_button11 = new JoystickButton(driveJoystick, 11),
            driveJs_button12 = new JoystickButton(driveJoystick, 12);

    public Button
        driveJs2_button3 = new JoystickButton(driveJoystick2, 3),
        driveJs2_button2 = new JoystickButton(driveJoystick2, 2),
        driveJs2_button4 = new JoystickButton(driveJoystick2, 4),
        driveJs2_button5 = new JoystickButton(driveJoystick2, 5);

    public Button 
            shooterJs_button1 = new JoystickButton(shooterJoystick, 1),
            shooterJs_button2 = new JoystickButton(shooterJoystick, 2),
            shooterJs_button3 = new JoystickButton(shooterJoystick, 3),
            shooterJs_button4 = new JoystickButton(shooterJoystick, 4),
            shooterJs_button5 = new JoystickButton(shooterJoystick, 5),
            shooterJs_button6 = new JoystickButton(shooterJoystick, 6),
            shooterJs_button7 = new JoystickButton(shooterJoystick, 7),
            shooterJs_button8 = new JoystickButton(shooterJoystick, 8),
            shooterJs_button9 = new JoystickButton(shooterJoystick, 9),
            shooterJs_button10 = new JoystickButton(shooterJoystick, 10),
            shooterJs_button11 = new JoystickButton(shooterJoystick, 11);
    
    public OI() {
        // Bind certain commands to certain buttons
        // This is the one of the few times that you will ever need to mess with the buttons
       shooterJs_button2.whileHeld(new ShooterSpin());
       shooterJs_button5.whileHeld(new ReversePickupLower());
       shooterJs_button7.whileHeld(new PickupBallsLower());

       driveJs_button3.whenPressed(new LockBridgeArm());
       driveJs_button4.whenReleased(new FindCameraDistance());
       driveJs_button5.whenPressed(new UnlockBridgeArm());
       driveJs_button11.whileHeld(new RaiseBridgeArm());
       driveJs_button12.whileHeld(new LowerBridge());
       
       driveJs2_button3.whileHeld(new SlowForward());
       driveJs2_button2.whileHeld(new SlowReverse());
       driveJs2_button4.whileHeld(new StrafeSlowLeft());
       driveJs2_button5.whileHeld(new StrafeSlowRight());

       //CommandBase.drivetrain.driveStraight();
    }
    
    public static OI getInstance() {
        if(instance == null) {
            instance = new OI();
        }
        
        return instance;
    }
    
    public Joystick getDriveJoystick() {
        return driveJoystick;
    }

    public Joystick getDriveJoystick2() {
        return driveJoystick2;
    }
    
    public Joystick getShooterJoystick() {
        return shooterJoystick;
    }
}

